/**
* @file matrix4x4.h
* July, 2010
* 
* 
* @author Antonio Miranda (amiranda@tecgraf.puc-rio.br )
* 
*/

#include <math.h>
#include ".\matrix4x4.h"

#define PI       3.14159265358979323846

//matrix4x4::matrix4x4(void)
//{
//}
//
//matrix4x4::~matrix4x4(void)
//{
//}

static inline bool qFuzzyIsNull(double d)
{
  return fabs(d) <= 0.000000000001;
}

//
//////////////////////////////////////////////////////////////////////////
matrix4x4::matrix4x4( const double *values )
{
  for (int row = 0; row < 4; ++row)
    for (int col = 0; col < 4; ++col)
      m[col][row] = values[row * 4 + col];
  flagBits = General;
}

//
//////////////////////////////////////////////////////////////////////////
matrix4x4::matrix4x4( const double *values, int cols, int rows )
{
  for (int col = 0; col < 4; ++col) {
    for (int row = 0; row < 4; ++row) {
      if (col < cols && row < rows)
        m[col][row] = values[col * rows + row];
      else if (col == row)
        m[col][row] = 1.0f;
      else
        m[col][row] = 0.0f;
    }
  }
  flagBits = General;
}

// Calculate the determinant of a 3x3 sub-matrix.
//     | A B C |
// M = | D E F |   det(M) = A * (EI - HF) - B * (DI - GF) + C * (DH - GE)
//     | G H I |
static inline double 
matrixDet3 (const double m[4][4], int col0, int col1, int col2,
            int row0, int row1, int row2)
{
  return m[col0][row0] *
    (m[col1][row1] * m[col2][row2] -
    m[col1][row2] * m[col2][row1]) -
    m[col1][row0] *
    (m[col0][row1] * m[col2][row2] -
    m[col0][row2] * m[col2][row1]) +
    m[col2][row0] *
    (m[col0][row1] * m[col1][row2] -
    m[col0][row2] * m[col1][row1]);
}

// Calculate the determinant of a 4x4 matrix.
static inline double matrixDet4(const double m[4][4])
{
  double det;
  det  = m[0][0] * matrixDet3(m, 1, 2, 3, 1, 2, 3);
  det -= m[1][0] * matrixDet3(m, 0, 2, 3, 1, 2, 3);
  det += m[2][0] * matrixDet3(m, 0, 1, 3, 1, 2, 3);
  det -= m[3][0] * matrixDet3(m, 0, 1, 2, 1, 2, 3);
  return det;
}

//
//////////////////////////////////////////////////////////////////////////
double matrix4x4::determinant() const
{
  return double(matrixDet4(m));
}

//
//////////////////////////////////////////////////////////////////////////
matrix4x4 matrix4x4::inverted( bool *invertible /*= 0*/ ) const
{
  // Handle some of the easy cases first.
  if (flagBits == Identity) {
    if (invertible)
      *invertible = true;
    return matrix4x4();
  } else if (flagBits == Translation) {
    matrix4x4 inv;
    inv.m[3][0] = -m[3][0];
    inv.m[3][1] = -m[3][1];
    inv.m[3][2] = -m[3][2];
    inv.flagBits = Translation;
    if (invertible)
      *invertible = true;
    return inv;
  } else if (flagBits == Rotation || flagBits == (Rotation | Translation)) {
    if (invertible)
      *invertible = true;
    return orthonormalInverse();
  }

  matrix4x4 inv(1); // The "1" says to not load the identity.

  double det = matrixDet4(m);
  if (det == 0.0f) {
    if (invertible)
      *invertible = false;
    return matrix4x4();
  }
  det = 1.0f / det;

  inv.m[0][0] =  matrixDet3(m, 1, 2, 3, 1, 2, 3) * det;
  inv.m[0][1] = -matrixDet3(m, 0, 2, 3, 1, 2, 3) * det;
  inv.m[0][2] =  matrixDet3(m, 0, 1, 3, 1, 2, 3) * det;
  inv.m[0][3] = -matrixDet3(m, 0, 1, 2, 1, 2, 3) * det;
  inv.m[1][0] = -matrixDet3(m, 1, 2, 3, 0, 2, 3) * det;
  inv.m[1][1] =  matrixDet3(m, 0, 2, 3, 0, 2, 3) * det;
  inv.m[1][2] = -matrixDet3(m, 0, 1, 3, 0, 2, 3) * det;
  inv.m[1][3] =  matrixDet3(m, 0, 1, 2, 0, 2, 3) * det;
  inv.m[2][0] =  matrixDet3(m, 1, 2, 3, 0, 1, 3) * det;
  inv.m[2][1] = -matrixDet3(m, 0, 2, 3, 0, 1, 3) * det;
  inv.m[2][2] =  matrixDet3(m, 0, 1, 3, 0, 1, 3) * det;
  inv.m[2][3] = -matrixDet3(m, 0, 1, 2, 0, 1, 3) * det;
  inv.m[3][0] = -matrixDet3(m, 1, 2, 3, 0, 1, 2) * det;
  inv.m[3][1] =  matrixDet3(m, 0, 2, 3, 0, 1, 2) * det;
  inv.m[3][2] = -matrixDet3(m, 0, 1, 3, 0, 1, 2) * det;
  inv.m[3][3] =  matrixDet3(m, 0, 1, 2, 0, 1, 2) * det;

  if (invertible)
    *invertible = true;
  return inv;
}

//
//////////////////////////////////////////////////////////////////////////
matrix4x4 matrix4x4::transposed() const
{
  matrix4x4 result(1); // The "1" says to not load the identity.
  for (int row = 0; row < 4; ++row) {
    for (int col = 0; col < 4; ++col) {
      result.m[col][row] = m[row][col];
    }
  }
  return result;
}

//
//////////////////////////////////////////////////////////////////////////
matrix4x4& matrix4x4::operator/=( double divisor )
{
  m[0][0] /= divisor;
  m[0][1] /= divisor;
  m[0][2] /= divisor;
  m[0][3] /= divisor;
  m[1][0] /= divisor;
  m[1][1] /= divisor;
  m[1][2] /= divisor;
  m[1][3] /= divisor;
  m[2][0] /= divisor;
  m[2][1] /= divisor;
  m[2][2] /= divisor;
  m[2][3] /= divisor;
  m[3][0] /= divisor;
  m[3][1] /= divisor;
  m[3][2] /= divisor;
  m[3][3] /= divisor;
  flagBits = General;
  return *this;
}

//
//////////////////////////////////////////////////////////////////////////
void matrix4x4::scale( double x, double y )
{
  if (flagBits == Identity) {
    m[0][0] = x;
    m[1][1] = y;
    flagBits = Scale;
  } else if (flagBits == Scale || flagBits == (Scale | Translation)) {
    m[0][0] *= x;
    m[1][1] *= y;
  } else if (flagBits == Translation) {
    m[0][0] = x;
    m[1][1] = y;
    flagBits |= Scale;
  } else {
    m[0][0] *= x;
    m[0][1] *= x;
    m[0][2] *= x;
    m[0][3] *= x;
    m[1][0] *= y;
    m[1][1] *= y;
    m[1][2] *= y;
    m[1][3] *= y;
    flagBits = General;
  }
}

//
//////////////////////////////////////////////////////////////////////////
void matrix4x4::scale( double x, double y, double z )
{
  if (flagBits == Identity) {
    m[0][0] = x;
    m[1][1] = y;
    m[2][2] = z;
    flagBits = Scale;
  } else if (flagBits == Scale || flagBits == (Scale | Translation)) {
    m[0][0] *= x;
    m[1][1] *= y;
    m[2][2] *= z;
  } else if (flagBits == Translation) {
    m[0][0] = x;
    m[1][1] = y;
    m[2][2] = z;
    flagBits |= Scale;
  } else {
    m[0][0] *= x;
    m[0][1] *= x;
    m[0][2] *= x;
    m[0][3] *= x;
    m[1][0] *= y;
    m[1][1] *= y;
    m[1][2] *= y;
    m[1][3] *= y;
    m[2][0] *= z;
    m[2][1] *= z;
    m[2][2] *= z;
    m[2][3] *= z;
    flagBits = General;
  }
}

//
//////////////////////////////////////////////////////////////////////////
void matrix4x4::scale( double factor )
{
  if (flagBits == Identity) {
    m[0][0] = factor;
    m[1][1] = factor;
    m[2][2] = factor;
    flagBits = Scale;
  } else if (flagBits == Scale || flagBits == (Scale | Translation)) {
    m[0][0] *= factor;
    m[1][1] *= factor;
    m[2][2] *= factor;
  } else if (flagBits == Translation) {
    m[0][0] = factor;
    m[1][1] = factor;
    m[2][2] = factor;
    flagBits |= Scale;
  } else {
    m[0][0] *= factor;
    m[0][1] *= factor;
    m[0][2] *= factor;
    m[0][3] *= factor;
    m[1][0] *= factor;
    m[1][1] *= factor;
    m[1][2] *= factor;
    m[1][3] *= factor;
    m[2][0] *= factor;
    m[2][1] *= factor;
    m[2][2] *= factor;
    m[2][3] *= factor;
    flagBits = General;
  }
}

//
//////////////////////////////////////////////////////////////////////////
void matrix4x4::translate( double x, double y )
{
  if (flagBits == Identity) {
    m[3][0] = x;
    m[3][1] = y;
    flagBits = Translation;
  } else if (flagBits == Translation) {
    m[3][0] += x;
    m[3][1] += y;
  } else if (flagBits == Scale) {
    m[3][0] = m[0][0] * x;
    m[3][1] = m[1][1] * y;
    m[3][2] = 0.;
    flagBits |= Translation;
  } else if (flagBits == (Scale | Translation)) {
    m[3][0] += m[0][0] * x;
    m[3][1] += m[1][1] * y;
  } else {
    m[3][0] += m[0][0] * x + m[1][0] * y;
    m[3][1] += m[0][1] * x + m[1][1] * y;
    m[3][2] += m[0][2] * x + m[1][2] * y;
    m[3][3] += m[0][3] * x + m[1][3] * y;
    if (flagBits == Rotation)
      flagBits |= Translation;
    else if (flagBits != (Rotation | Translation))
      flagBits = General;
  }
}

//
//////////////////////////////////////////////////////////////////////////
void matrix4x4::translate( double x, double y, double z )
{
  if (flagBits == Identity) {
    m[3][0] = x;
    m[3][1] = y;
    m[3][2] = z;
    flagBits = Translation;
  } else if (flagBits == Translation) {
    m[3][0] += x;
    m[3][1] += y;
    m[3][2] += z;
  } else if (flagBits == Scale) {
    m[3][0] = m[0][0] * x;
    m[3][1] = m[1][1] * y;
    m[3][2] = m[2][2] * z;
    flagBits |= Translation;
  } else if (flagBits == (Scale | Translation)) {
    m[3][0] += m[0][0] * x;
    m[3][1] += m[1][1] * y;
    m[3][2] += m[2][2] * z;
  } else {
    m[3][0] += m[0][0] * x + m[1][0] * y + m[2][0] * z;
    m[3][1] += m[0][1] * x + m[1][1] * y + m[2][1] * z;
    m[3][2] += m[0][2] * x + m[1][2] * y + m[2][2] * z;
    m[3][3] += m[0][3] * x + m[1][3] * y + m[2][3] * z;
    if (flagBits == Rotation)
      flagBits |= Translation;
    else if (flagBits != (Rotation | Translation))
      flagBits = General;
  }
}

//
//////////////////////////////////////////////////////////////////////////
void matrix4x4::rotate( double angle, double x, double y, double z /*= 0.0f*/ )
{
  if (angle == 0.0f)
    return;
  matrix4x4 m(1); // The "1" says to not load the identity.
  double c, s, ic;
  if (angle == 90.0f || angle == -270.0f) {
    s = 1.0f;
    c = 0.0f;
  } else if (angle == -90.0f || angle == 270.0f) {
    s = -1.0f;
    c = 0.0f;
  } else if (angle == 180.0f || angle == -180.0f) {
    s = 0.0f;
    c = -1.0f;
  } else {
    double a = angle * PI / 180.0f;
    c = cos(a);
    s = sin(a);
  }
  bool quick = false;
  if (x == 0.0f) {
    if (y == 0.0f) {
      if (z != 0.0f) {
        // Rotate around the Z axis.
        m.setToIdentity();
        m.m[0][0] = c;
        m.m[1][1] = c;
        if (z < 0.0f) {
          m.m[1][0] = s;
          m.m[0][1] = -s;
        } else {
          m.m[1][0] = -s;
          m.m[0][1] = s;
        }
        m.flagBits = General;
        quick = true;
      }
    } else if (z == 0.0f) {
      // Rotate around the Y axis.
      m.setToIdentity();
      m.m[0][0] = c;
      m.m[2][2] = c;
      if (y < 0.0f) {
        m.m[2][0] = -s;
        m.m[0][2] = s;
      } else {
        m.m[2][0] = s;
        m.m[0][2] = -s;
      }
      m.flagBits = General;
      quick = true;
    }
  } else if (y == 0.0f && z == 0.0f) {
    // Rotate around the X axis.
    m.setToIdentity();
    m.m[1][1] = c;
    m.m[2][2] = c;
    if (x < 0.0f) {
      m.m[2][1] = s;
      m.m[1][2] = -s;
    } else {
      m.m[2][1] = -s;
      m.m[1][2] = s;
    }
    m.flagBits = General;
    quick = true;
  }
  if (!quick) {
    double len = x * x + y * y + z * z;
    if (!qFuzzyIsNull(len - 1.0f) && !qFuzzyIsNull(len)) {
      len = sqrt(len);
      x /= len;
      y /= len;
      z /= len;
    }
    ic = 1.0f - c;
    m.m[0][0] = x * x * ic + c;
    m.m[1][0] = x * y * ic - z * s;
    m.m[2][0] = x * z * ic + y * s;
    m.m[3][0] = 0.0f;
    m.m[0][1] = y * x * ic + z * s;
    m.m[1][1] = y * y * ic + c;
    m.m[2][1] = y * z * ic - x * s;
    m.m[3][1] = 0.0f;
    m.m[0][2] = x * z * ic - y * s;
    m.m[1][2] = y * z * ic + x * s;
    m.m[2][2] = z * z * ic + c;
    m.m[3][2] = 0.0f;
    m.m[0][3] = 0.0f;
    m.m[1][3] = 0.0f;
    m.m[2][3] = 0.0f;
    m.m[3][3] = 1.0f;
  }
  int flags = flagBits;
  *this *= m;
  if (flags != Identity)
    flagBits = flags | Rotation;
  else
    flagBits = Rotation;
}

//
//////////////////////////////////////////////////////////////////////////
void matrix4x4::copyDataTo( double *values ) const
{
  for (int row = 0; row < 4; ++row)
    for (int col = 0; col < 4; ++col)
      values[row * 4 + col] = (m[col][row]);
}

// Helper routine for inverting orthonormal matrices that consist
// of just rotations and translations.
//////////////////////////////////////////////////////////////////////////
matrix4x4 matrix4x4::orthonormalInverse() const
{
  matrix4x4 result(1);  // The '1' says not to load identity

  result.m[0][0] = m[0][0];
  result.m[1][0] = m[0][1];
  result.m[2][0] = m[0][2];

  result.m[0][1] = m[1][0];
  result.m[1][1] = m[1][1];
  result.m[2][1] = m[1][2];

  result.m[0][2] = m[2][0];
  result.m[1][2] = m[2][1];
  result.m[2][2] = m[2][2];

  result.m[0][3] = 0.0f;
  result.m[1][3] = 0.0f;
  result.m[2][3] = 0.0f;

  result.m[3][0] = -(result.m[0][0] * m[3][0] + result.m[1][0] * m[3][1] + result.m[2][0] * m[3][2]);
  result.m[3][1] = -(result.m[0][1] * m[3][0] + result.m[1][1] * m[3][1] + result.m[2][1] * m[3][2]);
  result.m[3][2] = -(result.m[0][2] * m[3][0] + result.m[1][2] * m[3][1] + result.m[2][2] * m[3][2]);
  result.m[3][3] = 1.0f;

  return result;
}

//
///
void matrix4x4::transposed( double mat[4][4] ) const
{
  for (int row = 0; row < 4; ++row) 
  {
    for (int col = 0; col < 4; ++col) 
    {
      mat[col][row] = m[row][col];
    }
  }
}